clc;
clear;
val = 1/sqrt(3);
% 定义箭头的起点、终点和长度
x0 = 0; y0 = 0; z0 = 0;
dx = 1; dy = 1; dz = 1;

%一行是一种变量，传参传行，固定起点都为原点
axis_a = [zeros(3,3);eye(3)];
axis_b = [zeros(3,3);eye(3)];

% 绘制箭头并获取句柄
h_b = quiver3(axis_b(1,:),axis_b(2,:),axis_b(3,:),axis_b(4,:),axis_b(5,:),axis_b(6,:),'r','linewidth',2,'autoscalefactor',0.6);
text(-0.2,0,0,'B','fontsize',16,'color','r');
text(0.2,-0.1,0,'A','fontsize',16,'color','k');
hold on

% 固定坐标轴范围
axis([-0.5 1 -0.5 1 -0.5 1]);
daspect([1 1 1]);

% 定义旋转角度和旋转轴

%theta = linspace(0, pi/6, 1000);%这个弃用，因为不用线性插值
v = [val, val, val];
tmp = zeros(3,3);

%进行位置s曲线规划
t_acc = 1;
t_ed = 3;
delta_t = 0.01;
pos_st = 0;
pos_ed = pi/3;
pos_final = @(a)(a*t_acc^2+t_acc*(t_ed-a*t_acc)*a-pos_ed+pos_st);
a_value = fsolve(pos_final,0);
n = 0;
info = position_s_traj_info(a_value,t_acc,t_ed,delta_t,pos_st);
% 旋转箭头
for i = 1:delta_t:t_ed
    n = n+1;
    delete(findobj('type','text'));
    R = axang2rotm([v, info(n,1)]);
    for j = 1:3
        tmp(:,j) = R*axis_b(4:6,j);
    end
    h_b.UData = tmp(1,:);
    h_b.VData = tmp(2,:);
    h_b.WData = tmp(3,:);
    text(tmp(1,:),tmp(2,:),tmp(3,:),{'X_B','Y_B','Z_B'},'color','r');
    text(axis_a(4,:),axis_a(5,:),axis_a(6,:),{'X_A','Y_A','Z_A'});
    drawnow
    

    % 绘制静态坐标轴
    if i == 1
        quiver3(axis_a(1,:),axis_a(2,:),axis_a(3,:),axis_a(4,:),axis_a(5,:),axis_a(6,:),'k','linewidth',1);
    end
end
